#include "vehicle_ll_4d4s_mpc.h"
#include "poly2d.h"
#include <cmath>
namespace auto_ros
{
namespace control
{

VehicleLL4d4sMpc::VehicleLL4d4sMpc(const std::string dyn_mpc_kf_config_path, const std::string vehicle_lmpc_yaml_name, double rate)
	: VehicleLMpcBase(dyn_mpc_kf_config_path, vehicle_lmpc_yaml_name, rate)
{
	init_mpc();
} //
void VehicleLL4d4sMpc::init_mpc()
{

	dis_dyn_ptr_ = vehicle_dyn_ptr_->discretization(1 / rate_, "tustin");
	vehicle_mpc_ptr_ = std::make_shared<Mpc>(dis_dyn_ptr_, mpc_yaml_["mpc_windows"].as<int>(),
											 6 /*max_xu_constraint*/, 0, 2 /*max_delta_u*/, 0, 0,
											 mpc_yaml_["poly_num"].as<int>() * 2 /*max_xu_slack_ineq = 0*/, 0, 0, 0 /*max_delta_u_ae*/, 0, 0, 0);
	/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
					   const std::vector<double> u_index, const std::vector<double> u_factor,
					   int constraint_index, double min, double max)*/

	vehicle_mpc_ptr_->set_xu_affine({}, {}, {0}, {1.0}, 0, -mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14,
									mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {1}, {1.0}, 1, -mpc_yaml_["max_tire_r_angle"].as<double>() / 180 * 3.14,
									mpc_yaml_["max_tire_r_angle"].as<double>() / 180 * 3.14);
	std::vector<double> acc_max_vec = mpc_yaml_["acc_max"].as<std::vector<double>>();

	log_map_["ax_max_fl"] = acc_max_vec[0];
	log_map_["ax_max_fr"] = acc_max_vec[1];
	log_map_["ax_max_rl"] = acc_max_vec[2];
	log_map_["ax_max_rr"] = acc_max_vec[3];

	for (unsigned int tire_index = 0; tire_index < 4; tire_index++)
	{
		vehicle_mpc_ptr_->set_xu_affine({}, {}, {2 + tire_index}, {1.0}, 2 + tire_index, -acc_max_vec[tire_index],
										acc_max_vec[tire_index]);
	}

	update_friction_circle(1.0, 0, 0);
	//vehicle_mpc_ptr_->set_delta_u_ae(0, 0, 0);
	//vehicle_mpc_ptr_->set_delta_u_ae_w({mpc_yaml_["delta_tire_steering_f_w"].as<double>() * rate_ * rate_});
	//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
	vehicle_mpc_ptr_->set_delta_u(0, 0, -mpc_yaml_["max_steering_f_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_f_rate"].as<double>() / rate_);
	vehicle_mpc_ptr_->set_delta_u(1, 1, -mpc_yaml_["max_steering_r_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_r_rate"].as<double>() / rate_);
}
bool VehicleLL4d4sMpc::step(const Eigen::VectorXd &x0,
							const Eigen::VectorXd &u_fb,
							const PlanningRefPoint &ref_point,
							double speed, const std::map<std::string, double> other_input) // ax ay in map
{
	double ax;
	double ay;
	auto ax_iter = other_input.find("ax");
	if (other_input.end() != ax_iter)
	{
		ax = ax_iter->second;
	}
	else
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::calc_steady_state_ctr ax is not in other_par_map . \033[0m" << std::endl;
		throw 100;
	}
	auto ay_iter = other_input.find("ay");
	if (other_input.end() != ay_iter)
	{
		ay = ay_iter->second;
	}
	else
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::calc_steady_state_ctr ay is not in other_par_map . \033[0m" << std::endl;
		throw 100;
	}
	/////////////////////////////////////////////////////////////
	Eigen::VectorXd y(kf_lateral_ptr_->con_ptr_->out_dim());
	y << speed, x0(1), x0(2);
	Eigen::VectorXd control_last(kf_lateral_ptr_->con_ptr_->control_dim());
	control_last = u_fb;
	kf_lateral_ptr_->step(control_last, y, speed);
	Eigen::VectorXd state_esti = kf_lateral_ptr_->state_esti();

	Eigen::MatrixXd q_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->state_dim(), vehicle_dyn_ptr_->state_dim());
	q_temp(0, 0) = mpc_yaml_["longti_speed_factor"].as<double>();
	q_temp(1, 1) = mpc_yaml_["lateral_speed_factor"].as<double>();
	q_temp(2, 2) = mpc_yaml_["yaw_rate_factor"].as<double>();
	q_temp(3, 3) = mpc_yaml_["lateral_error_factor"].as<double>();
	q_temp(4, 4) = mpc_yaml_["heading_error_factor"].as<double>();
	Eigen::MatrixXd r_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->control_dim(), vehicle_dyn_ptr_->control_dim());
	r_temp(0, 0) = mpc_yaml_["steering_f_factor"].as<double>();
	r_temp(1, 1) = mpc_yaml_["steering_r_factor"].as<double>();
	for (unsigned int tire_index = 0; tire_index < 4; tire_index++)
	{
		r_temp(2 + tire_index, 2 + tire_index) = mpc_yaml_["acc_factor"].as<std::vector<double>>()[tire_index];
	}
	Eigen::VectorXd x_ref(dis_dyn_ptr_->state_dim());
	Eigen::VectorXd u_ref(dis_dyn_ptr_->control_dim());
	Eigen::VectorXd e_y_ref(dis_dyn_ptr_->out_dim());
	e_y_ref << 0.0, 0.0, 0.0;

	vehicle_dyn_ptr_->para_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)*/
	Eigen::VectorXd distur_ref(dis_dyn_ptr_->distur_dim());
	distur_ref << ref_point.kappa_, ref_point.a_, state_esti(3), state_esti(4), state_esti(5);
	//state_esti(3), state_esti(4), state_esti(5);

	//vehicle_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	std::map<std::string, double> other_par_map = {{"ax", ax}, {"ay", ay}};

	vehicle_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref, other_par_map);
	x_ref_ = x_ref;
	u_ref_ = u_ref;
	update_friction_circle(speed, ax, ay);
	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/
	Eigen::VectorXd temp_state_esti(6);
	temp_state_esti << speed, x0(1), x0(2), 0, 0, 0;

	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q_temp, r_temp, {}, {/**/},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLLYawMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
		reset_kf(temp_state_esti);
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		reset_kf(temp_state_esti);
		std::cout << "\033[31m Error:VehicleLLYawMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
void VehicleLL4d4sMpc::update_friction_circle(double speed, double ax, double ay)
{
	double speed_threshold = vehicle_dyn_ptr_->dyn_yaml_["speed_threshold"].as<double>();
	double speed_limited;
	if (std::abs(speed) < speed_threshold)
	{
		if (speed >= 0)
			speed_limited = speed_threshold;
		else
			speed_limited = -speed_threshold;
	}
	else
	{
		speed_limited = speed;
	}

	std::vector<double> ax_max_vec;
	calc_ax_max(ax, ay, ax_max_vec);
	log_map_["ax_max_fl"] = ax_max_vec[0];
	log_map_["ax_max_fr"] = ax_max_vec[1];
	log_map_["ax_max_rl"] = ax_max_vec[2];
	log_map_["ax_max_rr"] = ax_max_vec[3];
	int poly_num = mpc_yaml_["poly_num"].as<int>();
	std::vector<double> alpha_max_vec = mpc_yaml_["alpha_max"].as<std::vector<double>>();
	for (int i = 0; i < alpha_max_vec.size(); i++)
	{
		alpha_max_vec[i] = alpha_max_vec[i] / 180.0 * M_PI;
	}
	std::vector<double> tire_x_vec = {vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>(),
									  vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>(),
									  -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>(),
									  -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>()};

	std::vector<double> xu_slack_ineq_vec;
	//Mapping front friction circles to state and control spaces
	for (unsigned int tire_index = 0; tire_index < 4; tire_index++)
	{
		for (int i = 0; i < poly_num / 2; i++)
		{
			xu_slack_ineq_vec.push_back(mpc_yaml_["tire_friction_circle_w"]
											.as<std::vector<double>>()[tire_index]);
			double theta = 2 * M_PI / poly_num * i - 0.5 * M_PI;
			double theta1 = 2 * M_PI / poly_num * (i + 1) - 0.5 * M_PI;

			Point temp_point(ax_max_vec[tire_index] * std::cos(theta), alpha_max_vec[tire_index] * std::sin(theta));
			Point temp_point1(ax_max_vec[tire_index] * std::cos(theta1), alpha_max_vec[tire_index] * std::sin(theta1));
			LinearIneq temp_ineq(temp_point, temp_point1);
			//int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
			//				  const std::vector<double> u_index, const std::vector<double> u_factor,
			//				  int constraint_index, double min, double max);
			double coff_x = temp_ineq.coff_x();
			double coff_y = temp_ineq.coff_y();
			double coff_const = temp_ineq.coff_const();
			/*std::cout << "front tire constraints " << i << ":  "
				  << "coff_x:" << temp_ineq.coff_x() << "coff_y:"
				  << temp_ineq.coff_y() << "coff_const:" << temp_ineq.coff_const()
				  << " is_greater_than_zero:" << temp_ineq.is_greater_than_zero() << std::endl;*/
			//判断前后轴
			unsigned int axle_index = 0;
			if (tire_index >= 2)
			{
				axle_index = 1;
			}

			vehicle_mpc_ptr_->set_xu_slack_ineq({1, 2}, {coff_y / speed_limited, coff_y * tire_x_vec[tire_index] / speed_limited},
												{axle_index, 2 + tire_index}, {-coff_y, coff_x}, tire_index * 4 + i,
												-std::abs(coff_const),
												std::abs(coff_const));
		}
	}
	vehicle_mpc_ptr_->set_xu_slack_ineq_w(xu_slack_ineq_vec);
}
void VehicleLL4d4sMpc::calc_ax_max(double ax, double ay, std::vector<double> &ax_max_vec)
{
	ax_max_vec.clear();
	double lw = vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>();
	double h = vehicle_dyn_ptr_->dyn_yaml_["h"].as<double>();
	double lf = vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>();
	double lr = vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>();
	double road_mu = vehicle_dyn_ptr_->dyn_yaml_["road_mu"].as<double>();

	double l = lf + lr;
	double s_coff = g / 2.0 / l;
	double ax_coff = h / 2.0 / l;
	double ay_coff = h / 2.0 / lw / l;

	double az_fl = lr * (s_coff - ay * ay_coff) - ax * ax_coff;
	ax_max_vec.push_back(road_mu * az_fl);
	double az_fr = lr * (s_coff + ay * ay_coff) - ax * ax_coff;
	ax_max_vec.push_back(road_mu * az_fr);
	double az_rl = lf * (s_coff - ay * ay_coff) + ax * ax_coff;
	ax_max_vec.push_back(road_mu * az_rl);
	double az_rr = lf * (s_coff + ay * ay_coff) + ax * ax_coff;
	ax_max_vec.push_back(road_mu * az_rr);
}
bool VehicleLL4d4sMpc::reset_kf()
{
	Eigen::MatrixXd state_cov_esti =
		Eigen::MatrixXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim(),
							  kf_lateral_ptr_->con_ptr_->state_dim());
	Eigen::VectorXd state_esti =
		Eigen::VectorXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim());
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
bool VehicleLL4d4sMpc::reset_kf(const Eigen::VectorXd &state_esti)
{
	Eigen::MatrixXd state_cov_esti =
		Eigen::MatrixXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim(), kf_lateral_ptr_->con_ptr_->state_dim());
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
} // namespace control
} // namespace auto_ros
